

/**
 *
 * create by shjzhang, 2020/03/02
 *
 * @file CSDetectorUC.cpp
 *
 */

#include "CSDetectorUC.hpp"
#include "SatID.hpp"
#include "constants.hpp"

#define debug 1

namespace gpstk
{

      // Returns a string identifying this object.
   std::string CSDetectorUC::getClassName() const
   { return "CSDetectorUC"; }
    

      /* Returns a satTypeValueMap object, adding the new data generated
       *  when calling this object.
       *
       * @param epoch     Time of observations.
       * @param gData     Data object holding the data.
       * @param epochflag Epoch flag.
       */
   satTypeValueMap& CSDetectorUC::Process( const CommonTime& epoch,
                                           satTypeValueMap& gData)
      noexcept(false)
   {

      try
      {
          if(debug)
          {
              cout << "CSDetectorUC " << endl;
          }

          if(debug)
          {
              cout << "current epoch" << endl;
              gData.dump(cout,1);

              cout << "last epoch" << endl;
              stvDataPrev.dump(cout,1);
          }

          // let's compute the time-difference data for cycle-slip detection
          getEpochDiffData(gData);

          // store the satTypeValueMap
          stvDataPrev = gData;
    
          DeltaX deltax;

          if(pLCDetector!=NULL)
          {
              deltax = pLCDetector->getDeltaX();
          }
          else
          {
              cerr << "pLCDetector is NULL" << endl;
              exit(-1);
          }

          if(firstTime)
          {
              firstTime = false;
              return gData;
          }



             // Loop through all the satellites
          for (auto& stv: gData )
          {
              SatID sat = stv.first;

              if(sat.system != satSys)
              {
                  continue;
              }

              double deltaPrefit;
              try
              {
                  // Try to extract the values
                  deltaPrefit = (stv).second(diffPrefitType);
              }
              catch(...)
              {
                  continue;
              }

              /// get dx/dy/dz/dcdt deriative from satTypeValueMap
              /// Notice:
              /// L(t) - L(t-1) = rou - drou(t)*dx;
              double DX, DY, DZ, DT;
              try
              {
                 DX = (stv).second[TypeID::dStaX];
                 DY = (stv).second[TypeID::dStaY];
                 DZ = (stv).second[TypeID::dStaZ];
                 DT = (stv).second[TypeID::dcdtSta];
              }
              catch(Exception& e)
              {
                 std::cerr 
                     << getClassName() 
                     << ": TypeID not found" << std::endl;
                 exit(-1);
              }

              double deltaRou = DX*deltax.dx + DY*deltax.dy + DZ*deltax.dz + DT*deltax.dt;

              if(debug)
              {
                  cout << "sat:" << sat << endl;
                  cout << "deltaPrefit:" << deltaPrefit << endl;
                  cout << "deltaRou" << deltaRou << endl;
              }

              double diffIono;
              try
              {
                 diffIono = (stv).second[diffIonoType];
              }
              catch(Exception& e)
              {
                  continue;
              }

              if(debug)
              {
                  cout << "diffIono" << diffIono << endl;
              }


              double deltaL1 = deltaPrefit - deltaRou - diffIono;

              if(debug)
              {
                  cout << "deltaL1:" << deltaL1 << endl;
              }


              int csValue(0);
              int csFlag(0);
              csValue = std::floor(deltaL1/waveLength+0.5);

              if(debug)
                  cout << "csValue:" << csValue << endl;

              if(csValue > 0)
              {
                  // cycle-slip happened
                  stv.second[TypeID::CSL1] = 1;
              }
              else
              {
                  stv.second[TypeID::CSL1] = 0;
              }
          }

          // copy
          satTypeValueMap tempSatData = gData;

          TypeIDSet typeSet;

          typeSet.insert(TypeID::prefitL12);
          typeSet.insert(TypeID::prefitL15);
          typeSet.insert(TypeID::prefitL17);
          typeSet.insert(TypeID::prefitL18);
          typeSet.insert(TypeID::prefitL16);
          typeSet.insert(TypeID::prefitL27);
          typeSet.insert(TypeID::prefitL26);

          typeSet.insert(diffPrefitType);
          typeSet.insert(diffPostType);

          typeSet.insert(TypeID::CSL1);
          typeSet.insert(TypeID::CSLC);
          typeSet.insert(TypeID::CSWL);

          typeSet.insert(TypeID::interrupt);

          tempSatData.keepOnlyTypeID(typeSet);

          if(pCSDataStream!=NULL)
          {
              for(auto stv: tempSatData)
              {
                  (*pCSDataStream) 
                      << YDSTime(epoch) << " "
                      << stv.first << " "
                      << stv.second << " "
                      << endl;
              }
          }

          return gData;

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

   }  // End of method 'CSDetectorUC::Process()'


   void CSDetectorUC::getEpochDiffData(satTypeValueMap& sData)
   {
          // loop the satellite
       for(auto& stv: sData)
       {
           SatID sat = stv.first;
           if(sat.system != satSys)
           {
               continue;
           }

           if(stvDataPrev.find(sat) != stvDataPrev.end())
           {
               typeValueMap tvDataPrev = stvDataPrev[sat];

               bool hasCurrPrefit(false);
               if( stv.second.find(prefitType) != stv.second.end() )
               {
                   hasCurrPrefit = true;;
               }

               bool hasPrevPrefit(false);
               if( tvDataPrev.find(prefitType) != tvDataPrev.end() )
               {
                   hasPrevPrefit = true;;
               }

               double currPrefit, prevPrefit;
               if(hasCurrPrefit && hasPrevPrefit)
               {
                   currPrefit = stv.second(prefitType);
                   prevPrefit = tvDataPrev(prefitType);
                   stv.second[diffPrefitType] = currPrefit - prevPrefit;
               }
               else if( hasCurrPrefit && !hasPrevPrefit )
               {
                   // data break happened
                   stv.second[TypeID::CSL1] = 9.0;
               }
               else
               {
                   continue;
               }
           }
           else
           {
               stv.second[TypeID::CSL1] = 9.0;
           }
       }
   }

      /* Method to set the maximum interval of time allowed between two
       * successive epochs.
       *
       * @param maxDelta      Maximum interval of time, in seconds
       */
   CSDetectorUC& CSDetectorUC::setDeltaTMax(const double& maxDelta)
   {
         // Don't allow delta times less than or equal to 0
      if (maxDelta > 0.0)
      {
         deltaTMax = maxDelta;
      }
      else
      {
         deltaTMax = 31.0;
      }

      return (*this);

   }  // End of method 'CSDetectorUC::setDeltaTMax()'


      /* Returns a gnnsRinex object, adding the new data generated when
       * calling this object.
       *
       * @param gData    Data object holding the data.
       */
   void CSDetectorUC::Process(gnssRinex& gData)
      noexcept(false)
   {

      try
      {
         Process( gData.header.epoch, gData.body);

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

   }  // End of method 'CSDetectorUC::Process()'



}  // End of namespace gpstk
